#include "defaults.h"
#include "trianglemesh.h"
#include "utils.h"

STriangleMesh::STriangleMesh(size_t nodes, 
							 const std::vector< std::pair<std::string, Variant> >& vertexdata, 
							 const std::vector< std::pair<std::string, Variant> >& triangledata, 
							 const std::vector<math::Vec3ui>& triangleIndex) {
	Init(nodes, vertexdata, triangledata, triangleIndex);
}

STriangleMesh::STriangleMesh(const std::vector<Vec3n>& vertex, const std::vector< std::vector<size_t> >& index) {
	size_t nodes = vertex.size();
	std::vector< std::pair<std::string, Variant> > vertexdata;
	vertexdata.push_back(std::pair<std::string, Variant>("position", vertex));

	std::vector< std::pair<std::string, Variant> > triangledata;

	std::vector<math::Vec3ui> triangleIndex(index.size());
	for(size_t i = 0; i < triangleIndex.size(); i++) {
		triangleIndex[i] = math::V3ui(index[i][0], index[i][1], index[i][2]); 
	}

	Init(nodes, vertexdata, triangledata, triangleIndex);

	AddNodeAttribute<nfloat>("u", 0.0);
	AddNodeAttribute<nfloat>("v", 0.0);
	AddNodeAttribute<nfloat>("w", 0.0);
	AddNodeAttribute<Vec3n>("g0", math::V3n(0.0, 0.0, 0.0));
	AddNodeAttribute<Vec3n>("g1", math::V3n(0.0, 0.0, 0.0));
	AddNodeAttribute<size_t>("mark", 0);

	AddEdgeAttribute<size_t>("mark", 11);

	AddElementAttribute<Vec3n>("g0", math::V3n(0.0, 0.0, 0.0));
	AddElementAttribute<Vec3n>("g1", math::V3n(0.0, 0.0, 0.0));
	AddElementAttribute<size_t>("mark", 0);
}

STriangleMesh::STriangleMesh(const std::vector<Vec3n>& vertex, const std::vector<size_t>& index) {
	size_t nodes = vertex.size();
	std::vector< std::pair<std::string, Variant> > vertexdata;
	vertexdata.push_back(std::pair<std::string, Variant>("position", vertex));

	std::vector< std::pair<std::string, Variant> > triangledata;

	std::vector<math::Vec3ui> triangleIndex(index.size() / 3);
	for(size_t i = 0; i < triangleIndex.size(); i++) {
		triangleIndex[i] = math::V3ui(index[3 * i], index[3 * i + 1], index[3 * i + 2]); 
	}

	Init(nodes, vertexdata, triangledata, triangleIndex);

	AddNodeAttribute<nfloat>("u", 0.0);
	AddNodeAttribute<nfloat>("v", 0.0);
	AddNodeAttribute<nfloat>("w", 0.0);
	AddNodeAttribute<Vec3n>("g0", math::V3n(0.0, 0.0, 0.0));
	AddNodeAttribute<Vec3n>("g1", math::V3n(0.0, 0.0, 0.0));
	AddNodeAttribute<size_t>("mark", 0);

	AddEdgeAttribute<size_t>("mark", 11);

	AddElementAttribute<Vec3n>("g0", math::V3n(0.0, 0.0, 0.0));
	AddElementAttribute<Vec3n>("g1", math::V3n(0.0, 0.0, 0.0));
	AddElementAttribute<size_t>("mark", 0);
}

STriangleMesh::STriangleMesh(const std::vector<Vec3n>& vertex, const std::vector<math::Vec3ui>& index) {
	size_t nodes = vertex.size();
	std::vector< std::pair<std::string, Variant> > vertexdata;
	vertexdata.push_back(std::pair<std::string, Variant>("position", vertex));

	std::vector< std::pair<std::string, Variant> > triangledata;

	Init(nodes, vertexdata, triangledata, index);

	AddNodeAttribute<nfloat>("u", 0.0);
	AddNodeAttribute<nfloat>("v", 0.0);
	AddNodeAttribute<nfloat>("w", 0.0);
	AddNodeAttribute<Vec3n>("g0", math::V3n(0.0, 0.0, 0.0));
	AddNodeAttribute<Vec3n>("g1", math::V3n(0.0, 0.0, 0.0));
	AddNodeAttribute<size_t>("mark", 0);

	AddEdgeAttribute<size_t>("mark", 11);

	AddElementAttribute<Vec3n>("g0", math::V3n(0.0, 0.0, 0.0));
	AddElementAttribute<Vec3n>("g1", math::V3n(0.0, 0.0, 0.0));
	AddElementAttribute<size_t>("mark", 0);
}

STriangleMesh::~STriangleMesh() {
	for(size_t i = 0; i < vertex.size(); i++) delete vertex[i];
	for(size_t i = 0; i < edge.size(); i++) delete edge[i];
	for(size_t i = 0; i < triangle.size(); i++) delete triangle[i];
}

Node* STriangleMesh::AllocateVertex() {
	Node* v = new Node(nodeAttribute, vertexidcounter++);
	vertex.push_back(v);
	return v;
}

SEdge* STriangleMesh::AllocateEdge() {
	SEdge* e = new SEdge(edgeAttribute, edgeidcounter++);
	edge.push_back(e);
	return e;
}

STriangle* STriangleMesh::AllocateTriangle(Node* v0, Node* v1, Node* v2) {
	STriangle* t = new STriangle(v0, v1, v2, elementAttribute, triangleidcounter++);
	triangle.push_back(t);
	return t;
}

void STriangleMesh::Init(size_t nodes, 
						  const std::vector< std::pair<std::string, Variant> >& vertexdata, 
						  const std::vector< std::pair<std::string, Variant> >& triangledata, 
						  const std::vector<math::Vec3ui>& triangleIndex) {
	std::cout << "Initializing triangle mesh..." << std::endl;
	vertexidcounter = 0;
	edgeidcounter = 0;
	triangleidcounter = 0;

	size_t elements = triangleIndex.size();

	for(size_t i = 0; i < vertexdata.size(); i++) {
		const std::string& attribute = vertexdata[i].first;			
		const Variant& attributeData = vertexdata[i].second;
		nodeAttribute.Add(attribute, attributeData);
	}

	for(size_t i = 0; i < triangledata.size(); i++) {
		const std::string& attribute = triangledata[i].first;
		const Variant& attributeData = triangledata[i].second;
		elementAttribute.Add(attribute, attributeData);
	}

	for(size_t i = 0; i < nodes; i++) {
		Node* v = AllocateVertex();
	}

	mapEdge.clear();
	std::vector< std::list<size_t> > adj(nodes);
	for(size_t i = 0; i < elements; i++) {
		const math::Vec3ui& index = triangleIndex[i];

		size_t a = index[0];
		size_t b = index[1];
		size_t c = index[2];

		STriangle* t = AllocateTriangle(Vertex(a), Vertex(b), Vertex(c));
			
		adj[a].push_back(t->Id());
		adj[b].push_back(t->Id());
		adj[c].push_back(t->Id());
			
		//Edges
		std::vector< std::pair<size_t, size_t> > tempedge(3);
		tempedge[0].first = a;
		tempedge[0].second = b;
		tempedge[1].first = b;
		tempedge[1].second = c;
		tempedge[2].first = c;
		tempedge[2].second = a;
		for(size_t k = 0; k < 3; k++) {
			size_t i1 = tempedge[k].first;
			size_t i2 = tempedge[k].second;
				
			SEdge* e1 = AllocateEdge();
			e1->Init(Vertex(i1), Vertex(i2), t);
			Map(e1->a, e1->b, e1);
				
			t->edge[k] = e1;
			SEdge* e2 = LookUp(e1->b, e1->a);
			if(e2 != 0) {
				e1->neighbor = e2;
				e2->neighbor = e1;
			}
		}
	}
	std::cout << "triangles read." << std::endl;

//   	return;

	size_t isolated = 0;
	for(size_t i = 0; i < adj.size(); i++) {
		if(!adj[i].empty()) {
			SortNeighbors(Vertex(i), adj[i]);
		} else {
			std::cout << "problem!!!" << std::endl;
			Node* v = Vertex(i);
			v = 0;
			isolated++;
		}
	}
	/*for(size_t i = 0; i < adj.size(); i++) {
		Node* vi = Vertex(i);
		std::set<size_t> index;
		for(std::list<size_t>::iterator it = adj[i].begin(); it != adj[i].end(); ++it) {
			const STriangle* t = Triangle(*it);
			for(size_t j = 0; j < 3; j++) {
				const size_t id = t->vertex[j]->Id();
				if(id != vi->Id())
					index.insert(id);
			}
		}
		for(std::set<size_t>::iterator it = index.begin(); it != index.end(); ++it) {
			Node* vj = Vertex(*it);
			vi->AddNeighbor(vj);
		}
	}*/

	for(size_t i = 0; i < triangle.size(); i++) {
		Triangle(i)->Init("position");
	}
	
	bool hasBoundary = false;
	for(size_t i = 0; i < edge.size(); i++) {
		if(Edge(i)->neighbor == 0) {
			hasBoundary = true;
			break;
		}
	}

// 	hasBoundary = false;
	if(hasBoundary) {
		std::cerr << "triangle mesh has boundary." << std::endl;
		ComputeBoundary();
		const std::vector<Node*>& boundarypoint = Mesh::GetBoundary();
		std::list<Node*> boundary;
		for(size_t i = 0; i < boundarypoint.size(); i++) {
			boundary.push_back(static_cast<Node*>(boundarypoint[i]));
		}
		
		size_t j = 0;
		bool flag = true;
		while(!boundary.empty()) {
			boundaryn.push_back(std::vector<Node*>());
			
			Node* vStart = (*boundary.begin());
			Node* v = vStart, *vLast = vStart;
			do {
				if(!flag)
					break;
				for(size_t i = 0; flag && i < v->NumNeighbors(); i++) {
					Node* u = v->Neighbor(i);
					if(u->Id() != vLast->Id()) {
						SEdge* e = LookUp(u, v);
						if(!e)
							e = LookUp(v, u);
						if(!e->neighbor) {
							boundaryn[j].push_back(u);
							boundary.remove(u);
							vLast = v;
							v = u;
							break;
						}
					}
				}
			} while(v != vStart);
			boundary.remove(vStart);
			j++;
			if(!flag)
				break;
		}
		
		std::cerr << "found " << boundaryn.size() << " boundaries." << std::endl;
		for(size_t i = 0; i < boundaryn.size(); i++) {
			std::cerr << "boundary " << i + 1 << " has " << boundaryn[i].size() << " vertices." << std::endl;
		}
		std::cerr << "overall boundary vertices: " << boundarypoint.size() << std::endl;
	}
}

void STriangleMesh::SortNeighbors(Node* v, std::list<size_t>& trianglelist) {
	std::list<size_t>::iterator it;
	bool b = false;
	size_t tstartId = 0;
	
	for(it = trianglelist.begin(); it != trianglelist.end(); it++) {
		size_t id = static_cast<size_t>(*it);
		const STriangle& tri = *Triangle(id);
		for(size_t i = 0; i < 3; i++) {
			SEdge* e = tri.edge[i];
			if(e->a == v || e->b == v) {
				if(!e->neighbor) {
					b = true;
					tstartId = id;
					break;
				}
			}
		}
		if(b)
			break;
	}

	if(b) {
		std::set<Node*> neighbor;
        for(std::list<size_t>::iterator it = trianglelist.begin(); it != trianglelist.end(); it++) {
            size_t id = static_cast<size_t>(*it);
            const STriangle& tri = *Triangle(id);

            for(size_t i = 0; i < 3; i++) {
                if(tri.vertex[i] != v) {
                    neighbor.insert(tri.vertex[i]);
                }
            }
        }

        size_t i = 0;
		v->ResizeNeighbors(trianglelist.size() + 1);
        for(std::set<Node*>::iterator it = neighbor.begin(); it != neighbor.end(); it++)  {
            Node* n = static_cast<Node*>(*it);
            v->SetNeighbor(n, i++);
        }
	} else {
		v->ResizeNeighbors(trianglelist.size());
		it = trianglelist.begin();
		size_t id = static_cast<size_t>(*it);
		const STriangle& tri = *Triangle(id);
		for(size_t i = 0; i < 3; i++) {
			if(v == tri.vertex[i]) {
				v->SetNeighbor(tri.vertex[(i + 1) % 3], 0);
				v->SetNeighbor(tri.vertex[(i + 2) % 3], 1);
				break;
			}
		}

		size_t k = 2;
		Node* last = v->Neighbor(1);
		trianglelist.pop_front();

		it = trianglelist.begin();
		while(k < v->NumNeighbors()) {
			size_t id = static_cast<size_t>(*it);
			STriangle* tri = Triangle(id);
			size_t j = 0;
			for(j = 0; j < 3; j++) {
				if(v == tri->vertex[j]) {
					if(tri->vertex[(j + 1) % 3] == last) {
						v->SetNeighbor(tri->vertex[(j + 2) % 3], k);
						last = tri->vertex[(j + 2) % 3];
						break;
					}
				}
			}

			if(j != 3) {
				k++;
				trianglelist.remove(id);
				it = trianglelist.begin();
			} else
				it++;
		}
	}
}

SEdge* STriangleMesh::LookUp(Node* a, Node* b) {
	if(a == 0 || b == 0)
		return 0;
	
	std::pair<size_t, size_t> p(a->Id(), b->Id());

	return mapEdge[p];
}

bool STriangleMesh::Map(Node* a, Node* b, SEdge* e) {
	if(a == 0 || b == 0 || e == 0)
		return false;
	
	std::pair<size_t, size_t> p(a->Id(), b->Id());
	
	mapEdge[p] = e;

	return true;
}
 
bool STriangleMesh::UnMap(Node* a, Node* b) {
	if(a == 0 || b == 0)
		return false;
	
	std::pair<size_t, size_t> p(a->Id(), b->Id());

	mapEdge.erase(p);

	return true;
}

void STriangleMesh::Save(const std::string& filename, const std::string& attribute) const {
	std::ofstream file;
	file.open(filename.c_str());
	file << std::setprecision(8);
	file << "trianglemesh" << std::endl;
	file << SizeNodes() << std::endl;
	for(size_t i = 0; i < SizeNodes(); i++) {
		const Node* v = GetNode(i);
		const Vec3n& P = v->Get<Vec3n>(attribute);
		file << P[0] << " " << P[1] << " " << P[2] << std::endl; 
	}
	file << SizeElements() << std::endl;
	for(size_t i = 0; i < SizeElements(); i++) {
		const Element* e = GetElement(i);

		const size_t n = e->SizeNodes();
		for(size_t j = 0; j < e->SizeNodes(); j++) {
			const Node* v = e->GetNode(j);
			if(j == n - 1) file << v->Id(); else file << v->Id() << " ";
		}
		file << std::endl;
	}

	file.close();
}

void STriangleMesh::ComputeBoundary() {
	std::set<Node*> S;

	size_t n = GetNumEdges();
	for(size_t i = 0; i < n; i++) {
		SEdge* e = Edge(i);
		if(!e->neighbor) {
			S.insert(e->a);
			S.insert(e->b);
		}
	}

	for(std::set<Node*>::iterator it = S.begin(); it != S.end(); it++)
		boundary.push_back(*it);
}

std::vector<Node*> STriangleMesh::GetBoundary(size_t i) {
	if(boundaryn.empty() || i >= boundaryn.size())
		return std::vector<Node*>();
	
	return boundaryn[i];
}

Node* STriangleMesh::ExtractMin(const std::vector<nfloat>& D, std::vector<bool>& F) {
	size_t j;
	nfloat dmin = std::numeric_limits<nfloat>::max();

	for(size_t i = 0; i < GetNumVertices(); i++) {
		if(F[i]) {
			if(D[i] < dmin) {
				dmin = D[i];
				j = i;
			}	
		}
	}

	F[j] = false;
	return Vertex(j);
}

bool STriangleMesh::IsBoundaryVertex(Node* v) {
	for(size_t i = 0; i < v->NumNeighbors(); i++) {
		Node* u = v->Neighbor(i);
		SEdge* e = LookUp(u, v);
		if(!e)
			e = LookUp(v, u);
		if(!e->neighbor)
			return true;
	}
	
	return false;
}

size_t STriangleMesh::GetBoundaryIndex(Node* v) const {
	for(size_t i = 0; i < boundaryn.size(); i++) {
		for(size_t j = 0; j < boundaryn[i].size(); j++) {
			if(boundaryn[i][j] == v)
				return i;
		}
	}
	std::cerr << "error: vertex is no boundary vertex." << std::endl;
	return 0;
}

Node* STriangleMesh::Find(const Vec3n& P, const std::string& attribute, const nfloat e) {
	for(size_t i = 0; i < GetNumVertices(); i++) {
		Node* v = Vertex(i);
		if(length(v->Get<Vec3n>(attribute) - P) < e)
			return v;
	}
	return 0;
}

Node* STriangleMesh::FindClosest(const Vec3n& P, const std::string& attribute) {
	nfloat min = std::numeric_limits<nfloat>::max();

	Node* vmin;
	size_t n = GetNumVertices();
	for(size_t i = 0; i < n; i++) {
		Node* v = Vertex(i);
		nfloat d = length(v->Get<Vec3n>(attribute) - P);
		if(d < min) {
			vmin = v;
			min = d;
		}
	}
	return vmin;
}

bool STriangleMesh::Find(std::vector<Node*>& Q, const Vec3n& P, const std::string& attribute, nfloat e) {
	Q.clear();
	for(size_t i = 0; i < GetNumVertices(); i++) {
		Node* v = Vertex(i);
		if(length(v->Get<Vec3n>(attribute) - P) < e)
			Q.push_back(v);
	}

	return !Q.empty();
}

std::vector<STriangle*> STriangleMesh::GetRing(Node* v) {
	std::vector<STriangle*> t;
	std::set<STriangle*> s;

	for(size_t i = 0; i < v->NumNeighbors(); i++) {
		Node* n = v->Neighbor(i);

		SEdge* e = LookUp(v, n);
		if(!e)
			e = LookUp(n, v);
		
		if(e) {
			if(e->triangle)
				s.insert(e->triangle);
	
			if(e->neighbor && e->neighbor->triangle)
				s.insert(e->neighbor->triangle);
		}
	}

	for(std::set<STriangle*>::iterator it = s.begin(); it != s.end(); it++) {
		t.push_back(static_cast<STriangle*>(*it));
	}

	return t;
}

void STriangleMesh::GetRing(Node* v, std::vector<SEdge*>& e) {
	std::set<SEdge*> s;

	for(size_t i = 0; i < v->NumNeighbors(); i++) {
		Node* n = v->Neighbor(i);

		SEdge* se = LookUp(v, n);
		if(se)
			s.insert(se);
		else
			std::cerr << "GetRing_Edge: error." << std::endl;
	}

	e.clear();
	for(std::set<SEdge*>::iterator it = s.begin(); it != s.end(); it++) {
		e.push_back(static_cast<SEdge*>(*it));
	}
}
	
nfloat STriangleMesh::Area(Node* v, const std::string& attribute) {
	const std::vector<STriangle*> ring = GetRing(v);

	nfloat A = 0.0;
	for(size_t i = 0; i < ring.size(); i++)
		A += ring[i]->Area(attribute);

	return A;
}
